#include "cmd.h"



/*接收缓冲器*/
static int cmd; 
static char recvData[100];
static int datalen;

/// @brief 检查是否获取到新命令并执行命令对应的功能函数, 在主循环中调用, 非阻塞
/// @param  
void cmd_check(void)
{
    /*检查是否收到来自上位机的命令并执行对应操作*/
    recvPacket(0, &cmd, recvData, &datalen);
    doCmd dohandle =  getCmd(cmd);
    if(dohandle != NULL){
        dohandle(recvData, datalen);
        cmd = 0;			
    }

    /*定时检查电机位置并上报*/
    if(counter > 100 && true){ // 1 counter = 20ms = 0.02s
        //delay_ms(25); //防止吞指令
        motorUpdatePose(1);
        counter = 0;
    }
}

/// @brief 插入对应命令
/// @param  
void cmd_init(void)
{
    insetCmd(CMD_TEST, testCmd);
    insetCmd(CMD_MOTOR_POSE, motorPoseCmd);
    insetCmd(CMD_MOTOR_SERVO, motorServo);
}

/// @brief 用于测试连通性, 会返回收到的data
/// @param data 
/// @param len 
/// @return 
int testCmd(char *data, int len)
{
    sendPacket(0, 0x1, data, len);
    return 0;
}

u8 cl = 0;
void t3_call(void)
{
    cl += 10;
    return ;
}

void test()
{
    int res;
    int pose ; 
    int vel ;
    int dir ; // 0 正转 1 反转
    
//    printf("hello \r\n");
//    u8 data[5] = "hello";
//    sendPacket(0,0x01,data,5);
	
    /* 一号电机 */
//    pose = 0; // 190000
//    vel = 5000;
//    dir = 0;
//    Emm_V5_Pos_Control(1, dir, vel, 0, pose, true, false);
//    while(can.rxFrameFlag == false); can.rxFrameFlag = false;delay_ms(5);
//	delay_ms(50);
    
    /*二号电机8*/
    pose = 0; // 100000
    vel = 5000;
    Emm_V5_Pos_Control(2,0,vel,0,pose,true,false);

   
    /*三号和四号电机*/
//    dir = 1;
//	vel = 800;
//	u8 acc = 0;
//	u32 step = 20300; // 最高点320000  复位值 3200*3 = 9600 第一个格子 20000
//    
////    static uint32_t STEP_Y0_1 = 23000; // 第一个格子对应的高度的步进 左边
////    static uint32_t STEP_DY_1 = 25000; // 每个格子的高度间隔 右边
////    int y = 5;
////    step = STEP_Y0_1 + y*STEP_DY_1; 
//    
//    //Emm_V5_Vel_Control(3, 1, 100, 0, true);
//    Emm_V5_Pos_Control(3,dir,vel,acc,step,true,true);
//	res = waitSMResponse(50); if(res == 1) return ;
//	delay_ms(50);
//	//Emm_V5_Vel_Control(4, 1, 100, 0, true);
//	Emm_V5_Pos_Control(4,dir,vel,acc,step,true,true);
//	res = waitSMResponse(50); if(res == 1) return ;
//	delay_ms(50);
//	Emm_V5_Synchronous_motion(0);

    /*五号电机*/
//    pose = 0; // 3200*36.5 = 116800
//    dir = 0;
//    vel = 5000;
//    Emm_V5_Pos_Control(5, dir, vel, 0, pose, true, false);

//    /*六号电机*/
//    pose = 0; // 118400
//    dir = 0;
//    vel = 5000;
//    Emm_V5_Pos_Control(6, dir, vel, 0, pose, true, false);


    //Servo1_SetAngle(180);
//    while(1){
//        Servo2_SetAngle(100);
//        delay_ms(2000);
//        Servo2_SetAngle(30);
//        delay_ms(2000);
//    }
//    
    
    /*360舵机*/
    
    
//    Servo1_Set();
//    while(MotorPose.S1 == 0);
//    delay_ms(2000);
//    Servo1_Reset();
}
